/*
 *******************
 ******************************* C SOURCE FILE *******************************
 **                                                      *******************                                               **
 **                                                                                                                                                **
 ** project : ClockSync                                                                                                    **
 ** filename :Timer                                                                                                                        **
 ** version : 1                                                                                                                            **
 ** date : -    2012.04.10.                                                                                                        **
 **                                                                                                                                                **
 *****************************************************************************
 **                                                                                                                                                **
 ** Copyright (c) 2012,                                                                                                        **
 ** All rights reserved.                                                                                                           **
 **                                                                                                                                                **
 *****************************************************************************
 VERSION HISTORY:
 ----------------
 Version : 1
 Date : -
 Revised by : -
 Description : Original version.
 */
#define _USERTASKS_C_SRC
/****************************************************************************/
/**                                                                                                                                        **/
/**                                                     MODULES USED                                                       **/
/**                                                                                                                                        **/
/****************************************************************************/

#include <cyg/hal/hal_io.h>
#include <math.h>
#include <stdlib.h>

#include <com_r04_s01b.h>
#include "platform.h"

#include "UserTasks.h"
#include "Timer.h"
/****************************************************************************/
/**                                                                                                                                        **/
/**                                                     DEFINITIONS AND MACROS                                     **/
/**                                                                                                                                        **/
/****************************************************************************/

/****************************************************************************/
/**                                                                                                                                        **/
/**                                                     TYPEDEFS AND STRUCTURES                                    **/
/**                                                                                                                                        **/
/****************************************************************************/

/****************************************************************************/
/**                                                                                                                                        **/
/**                                                     PROTOTYPES OF LOCAL FUNCTIONS                      **/
/**                                                                                                                                        **/
/****************************************************************************/

/* handle the incoming messages from the RF chip */
static void process_message(Packet_t* packet, cyg_uint16 packet_length);

/* request the devices clock in the network  periodically*/
static void process_clock_sync_request(Packet_t* packet);

/* Answer the other devices request */
static void process_clock_sync_asnwer(Packet_t* packet);

/****************************************************************************/
/**                                                                                                                                        **/
/**                                                     EXPORTED VARIABLES                                                 **/
/**                                                                                                                                        **/
/****************************************************************************/

cyg_uint8 NODE_ID;
cyg_flag_t Timed_clock_sync_event;
cyg_mutex_t Com_layer_send_mutex;

int Timed_clock_sync_request_stack[TASK_STACK_SIZE];
cyg_handle_t Timed_clock_sync_request_handle;
cyg_thread Timed_clock_sync_request_request_obj;

int Timed_clock_sync_answer_stack[TASK_STACK_SIZE];
cyg_handle_t Timed_clock_sync_answer_handle;
cyg_thread Timed_clock_sync_answer_obj;

int Common_recieve_stack[TASK_STACK_SIZE];
cyg_handle_t Common_recieve_handle;
cyg_thread Common_recieve_obj;
/****************************************************************************/
/**                                                                                                                                        **/
/**                                                     GLOBAL VARIABLES                                                   **/
/**                                                                                                                                        **/
/****************************************************************************/
static cyg_uint8 requester_id;
static SyncClock_t Sync_clock;
/****************************************************************************/
/**                                                                                                                                        **/
/**                                                     EXPORTED FUNCTIONS                                                 **/
/**                                                                                                                                        **/
/****************************************************************************/

void Common_recieve(cyg_addrword_t index)
{
        Packet_t packet;
        cyg_uint8 ret_val;

        /* the maximum length of packet */
        cyg_uint16 length;
        static cyg_uint32 error_counter;

        while (1)
        {
                /*
                 *  receive with timeouts possible return values
                 * COM_R04_S01__ERROR - crc error  / wrong packet
                 * COM_R04_S01__TIMEOUT - no packet received
                 * COM_R04_S01__NOERROR - packet received correctly
                 */

                /* set the maximum length */
                length = 100;

                /* check if receive unsuccessful */
                cyg_mutex_lock(&Com_layer_send_mutex);

                ret_val = com_r04_s01__Receiving_Packet_with_timeout((char*) &packet,
                                &length, 300);

                cyg_mutex_unlock(&Com_layer_send_mutex);
                switch (ret_val)
                {
                /*  crc or length error */
                case COM_R04_S01__ERROR:
                {
                        diag_printf(" Error while receiving %u.th time! \r\n\r\n",
                                        error_counter++);
                        break;
                }
                        /* no packet in the buffer */
                case COM_R04_S01__TIMEOUT:
                {
                        break;
                }

                        /* receive the packet */
                case COM_R04_S01__NOERROR:
                {
                        process_message(&packet, length);
                        break;
                }
                default:
                {
                        diag_printf(" Unexpected return value from receive %u ! \r\n\r\n",
                                        ret_val);
                        break;
                }
                }
        }
}

void Timed_clock_sync_answer(cyg_addrword_t index)
{
        cyg_flag_value_t flag_value;
        Packet_t packet;
        diag_printf(" timed_clock_sync_answer started\r\n\r\n");

        while (1)
        {
                /*
                 * CYG_FLAG_WAITMODE_AND
                 *return match if all conditions in the pattern are set
                 *
                 * CYG_FLAG_WAITMODE_CLR
                 * automatically clear the flag IF there was a match
                 */
                flag_value = cyg_flag_wait(&Timed_clock_sync_event,
                                TIMED_CLOCK_SYNC_ANSWER_EVENT, CYG_FLAG_WAITMODE_AND
                                                | CYG_FLAG_WAITMODE_CLR);

                /* Making sure, that cyg_flag_wait()
                 * has been returned because of the event
                 * and not because of a cyg_thread_release() call
                 */

                if (flag_value & TIMED_CLOCK_SYNC_ANSWER_EVENT)
                {
                        diag_printf(" valasz kuldve\r\n\r\n");

                        /* send the current time and the clock error */
                        packet.Clock = Get_time();

                        /* if the clock error is initialized send ERROR_INIT_VALUE
                         * otherwise send the calculate the clock error and send it
                         */

                        if (!Sync_clock.Is_initialized)
                        {
                                packet.ClockError = ERROR_INIT_VALUE;
                        }
                        else
                        {
                                packet.ClockError = Sync_clock.H + (packet.Clock
                                                - Sync_clock.Rho) * abs(Sync_clock.Drift);
                        }

                        packet.ClockError = packet.MessageType = CLOCK_ASNWER;
                        packet.SourceId = NODE_ID;
                        packet.DestinationID = requester_id;

                        /* send the packet */
                        cyg_mutex_lock(&Com_layer_send_mutex);
                        com_r04_s01__Send_Packet((unsigned char*) &packet, PACKET_SIZE);
                        cyg_mutex_unlock(&Com_layer_send_mutex);
                }
                else
                {
                        diag_printf("wait on my_flag interrupted\r\n");
                }
        }
}
void Timed_clock_sync_request(cyg_addrword_t index)
{
        cyg_flag_value_t flag_value;
        Packet_t packet;

        diag_printf(" timed_clock_sync_request started\r\n\r\n");

        while (1)
        {
                /*
                 * CYG_FLAG_WAITMODE_AND
                 *return match if all conditions in the pattern are set
                 *
                 * CYG_FLAG_WAITMODE_CLR
                 * automatically clear the flag IF there was a match
                 */
                flag_value = cyg_flag_wait(&Timed_clock_sync_event,
                                TIMED_CLOCK_SYNC_REQUEST_EVENT, CYG_FLAG_WAITMODE_AND
                                                | CYG_FLAG_WAITMODE_CLR);

                /* Making sure, that cyg_flag_wait()
                 * has been returned because of the event
                 * and not because of a cyg_thread_release() call
                 */

                if (flag_value & TIMED_CLOCK_SYNC_REQUEST_EVENT)
                {

                        diag_printf(" flag_value\r\n\r\n");

                        packet.SourceId = NODE_ID;
                        packet.DestinationID = 0;
                        packet.MessageType = CLOCK_REQUEST;
                        packet.ClockError = 0;
                        packet.Clock = 0;

                        /* send the packet */
                        cyg_mutex_lock(&Com_layer_send_mutex);
                        com_r04_s01__Send_Packet((unsigned char*) &packet, PACKET_SIZE);
                        cyg_mutex_unlock(&Com_layer_send_mutex);

                        /* set the next request time,
                         * if no device request clock it will elapse after 2 REQUEST_TIME_SLOT,
                         * if a device request clock match register will be set for that time
                         */
                        Set_request_timer(REQUEST_TIME_SLOT* NODE_COUNT * 2);

                        /* for demo*/
                        //TODO remove
                        //Set_answer_timer(NODE_ID * ANSWER_TIMES_SLOT);
                }
                else
                {
                        diag_printf("wait on my_flag interrupted\r\n");
                }
        }

}
void Init_sync_clcok(void)
{
        Sync_clock.Drift = 0.0;
        Sync_clock.H = 500 * NODE_ID;
        Sync_clock.Is_initialized = false;
        Sync_clock.Request_sent_time = 0;
        Sync_clock.Rho = 0;
}
/****************************************************************************/
/**                                                                                                                                        **/
/**                                                     LOCAL FUNCTIONS                                                    **/
/**                                                                                                                                        **/
/****************************************************************************/

static void process_clock_sync_asnwer(Packet_t* packet)
{
        cyg_uint32 current_time; // current time
        cyg_int32 delta_time; // time since request
        cyg_int32 transmit_delay; // data transmission delay
        cyg_int32 own_error;
        cyg_int32 sender_error; // clock error
        cyg_uint32 new_clock;
        cyg_uint32 clock_modification;

        //TODO calulate the clock modification
        //return;
        current_time = Get_time();

        /* the time between the request send time and now */
        delta_time = current_time - Sync_clock.Request_sent_time;

        /* the time while the message arrives from the sender */
        transmit_delay = delta_time - (ANSWER_TIMES_SLOT * packet->SourceId) / 2;

        /* calculate own error */
        own_error = Sync_clock.H + abs(delta_time * Sync_clock.Drift);

        /* calculate sender error */
        sender_error = packet->ClockError + abs(transmit_delay * (1
                        + Sync_clock.Drift));

        /* change clock if our clock accuracy is worse or if we are new and the other is not */
        if ((abs(sender_error) < abs(own_error)) || (Sync_clock.Is_initialized
                        == false && packet->ClockError != ERROR_INIT_VALUE))
        {
                /* update the clock */
                new_clock = packet->Clock;

                current_time = Get_time();

                /* calculate the difference between the correct time */
                clock_modification = new_clock - current_time;

                /* update the match registers */
                //Update_request_timer(clock_modification);
                //Update_answer_timer(clock_modification);

                /* set the new clocks */
                Set_time(new_clock);

                /* sava the last correction time */
                Sync_clock.Rho = new_clock;

                /* calulate the base error */
                Sync_clock.H = sender_error + (1 + Sync_clock.Drift) * transmit_delay;

                /* calclate the drift with exponential average */
                Sync_clock.Drift = 0.9 * Sync_clock.Drift
                                + 0.1 * clock_modification / (NODE_COUNT* REQUEST_TIME_SLOT);

                /* print the changes */
                diag_printf("C=%d, E=%d change=%d\r\ndrift = %d\r\n", new_clock,
                                Sync_clock.H, clock_modification, (int) Sync_clock.Drift);
        }
        else
        {
                /* if the senders error not better than the device`s */

                /* save the last correceted time */
                Sync_clock.Rho = current_time;

                /* calulate the base error */
                Sync_clock.H = own_error / 10 * 9 + CLK_PRECISION * (NODE_COUNT
                                * REQUEST_TIME_SLOT / 1000000) / 10;
                /*calculate the drift */
                Sync_clock.Drift = 0.9 * Sync_clock.Drift;

                /* print the changes */
                diag_printf("C=%d, E=%d no change\r\n drift = %d\r\n", current_time,
                                Sync_clock.H, (int) Sync_clock.Drift);
        }

        Sync_clock.Is_initialized = true;

}

static void process_message(Packet_t* packet, cyg_uint16 packet_length)
{
        diag_printf(
                        " level received from SourceID: %u   DestinationID:%u  MessageType:%u ! \r\n\r\n",
                        packet->SourceId, packet->DestinationID, packet->MessageType);
        switch (packet->MessageType)
        {
        case CLOCK_REQUEST:
        {
                process_clock_sync_request(packet);
                break;
        }
        case CLOCK_ASNWER:
        {

                process_clock_sync_asnwer(packet);
                break;
        }
        default:
        {
                diag_printf(
                                " Bad message received from SourceID: %u   DestinationID:%u  MessageType:%u ! \r\n\r\n",
                                packet->SourceId, packet->DestinationID, packet->MessageType);
                break;
        }
        }
}

void process_clock_sync_request(Packet_t* packet)
{

        Set_request_timer(((NODE_COUNT + NODE_ID - packet->SourceId) % NODE_COUNT)
                        * REQUEST_TIME_SLOT);

        Set_answer_timer(NODE_ID * ANSWER_TIMES_SLOT);
        requester_id = packet->SourceId;
}

/****************************************************************************/
/**                                                                                                                                        **/
/**                                                     EOF                                                                        **/
/**                                                                                                                                        **/
/****************************************************************************/

